#include "GeoUtil.h"
#include "core/CMath.h"

/**
* @brief Conversion of real scene coordinate system and data to GL coordinate system: +X to the left => x=-x:GL(+X to the right), +Y upwards => y=-z:GL(-Z forward)
* Curve: x = -L - [c0 + c1 * Sz * (-z + D) + c2 * Sz^2 * (-z + D)^2 + c3 * Sz^3 * (-z + D)^3] * Sx;
* 
* Scale First, Move Second <=> x = -L - c0 * Sx - [c1 * Sz * (-z + D) + c2 * Sz^2 * (-z + D)^2 + c3 * Sz^3 * (-z + D)^3] * Sx;
* 
* #1 Scaling first, then translating!
* Sx: Sx < 1, horizontal shrink
* Sz: Sz < 1, vertical shrink
* L: L > 0, horizontal shift to the left
* D: D > 0, vertical shift downwards
* 
* #2 Translating first, then scaling:
* x = [-L - c0 - c1 * Sz * (-z + D) - c2 * Sz^2 * (-z + D)^2 - c3 * Sz^3 * (-z + D)^3] * Sx;
* dz/dx = 1 / (dx/dz) = -1 / {[c1 * Sz + 2 * c2 * Sz^2 * (-z + D) + 3 * c3 * Sz^3 * (-z + D)^2] * Sx};
* Other rules are the same as before.
* 
* @note Whether in the XY plane or the XZ plane, the positive or negative slope will correctly reflect the direction of the tangent.
* @note because of scale the X and Y axis, coordX is not linear change with c0.example: c0 move delta, coordX move delta/Sx. means:scale + movement, not just movement
* 
* @param z: real 3DView CoordSys z value
*/
void GeoUtil::getByEquation(const LineEq& eq, float z, float* x, float* dx) {
	ASSERT(eq.isValid(), "LineEq is not valid!");

	float c0 = eq.C[0], c1 = eq.C[1], c2 = eq.C[2], c3 = eq.C[3];
	if (x != nullptr)
		*x = - (c0 + c1 * (-z) + c2 * pow(-z, 2) + c3 * pow(-z, 3));
	if (dx != nullptr)
		*dx = -1 / (c1 + 2 * c2 * (-z) + 3 * c3 * pow(-z, 2));// angle: quadrants:1 & 4
}

void GeoUtil::createSolidLine(CVector<P>& outVertices, const LineEq& eq) {
	CVector<P> vertices;
	createOneDimensionalSolidLinePoints(vertices, eq);

	const auto& Width = Config::Line_Width;
	for (const auto& center : vertices) {
		auto xL = center.x - Width / 2;
		auto xR = center.x + Width / 2;
		outVertices << P(xL, center.z) << P(xR, center.z);
	}
}

void GeoUtil::createDashedLine(CVector<P>& outVertices, CVector<UInt>& outIndices, const LineEq& eq, float slopeRatioByWidth, float vOffset) {
	const auto& SegLen = Config::Line_Dashed_SegLen;
	const auto& SegInterval = Config::Line_Dashed_SegInterval;

	CVector<CVector<P>> verticesList;
	createOneDimensionalDashedLinePoints(verticesList, eq, SegLen, SegInterval, Config::Line_PointsZStep, vOffset);/** @todo shortter segments may should use less points*/

	const auto& Width = Config::Line_Width;
	int m = 0;
	for (const auto& points : verticesList) {
		for (int i = 0; i < points.size(); i++) {
			const auto& center = points[i];
			auto xL = center.x - Width / 2;
			auto xR = center.x + Width / 2;
			auto pL = P(xL, center.z);
			auto pR = P(xR, center.z);
			if (i == 0 && slopeRatioByWidth != 0) {
				auto dx = getDxByEquation(eq, center.z);
				outVertices << getPointOnVector(slopeRatioByWidth > 0 ? pL : pR, std::atan(dx), std::abs(slopeRatioByWidth) * Width, false);
				auto n = outVertices.size();
				outIndices << n - 1 << n + 1 << n;
				/* 3  4
				 * 1  2
				 * 0/
				 * will be: 021,312,324
				 */
			}
			if (i >= 1) {
				if (i == 1)
					m = outVertices.size();
				/**
				 *@note just print indices to identify if is correct logic
				 * ... Pn-2 Pn-1 Pn
				 * 4  -  5
				 * 2 | | 3
				 * 0  -  1
				 * P0 P1 P2D ...
				 */
				outIndices << i*2 - 2 + m << i*2 - 4 + m << i*2 - 3 + m;// 201,423,645,... => firstIndex = verticesSize
				outIndices << i*2 - 2 + m << i*2 - 3 + m << i*2 - 1 + m;// 213,435,657,...
			}
			outVertices << pL << pR;
			if (i == points.size() - 1 && slopeRatioByWidth != 0) {
				auto dx = getDxByEquation(eq, center.z);
				outVertices << getPointOnVector(slopeRatioByWidth > 0 ? pR : pL, std::atan(dx), std::abs(slopeRatioByWidth) * Config::Line_Width);
				auto n = outVertices.size();
				outIndices << n - 3 << n - 2 << n - 1;
			}
		}
	}
}

void GeoUtil::createOneDimensionalSolidLinePoints(CVector<P>& outVertices, const LineEq& eq, float vStep) {
	/** @note zStart and zEnd must be scaled to View3D size data before use!!! */
	ASSERT(eq.zEnd <= eq.zStart, "Error occur: should zEnd <= zStart!");
	for (float z = eq.zStart; z >= eq.zEnd; z -= vStep) {
		auto x = getXByEquation(eq, z);
		outVertices << P(x, z);
	}
}

void GeoUtil::createOneDimensionalDashedLinePoints(CVector<CVector<P>>& outVerticesList, const LineEq& eq, float segLen, float segInterval, float vStep, float vOffset) {
	/** @note zStart and zEnd must be scaled to View3D size data before use!!! */
	ASSERT(eq.zEnd <= eq.zStart, "Error occur: should zEnd <= zStart!");
	float length = vOffset;
	P lastP;
	int m = 0;
	CVector<P> linePoints;
	for (float z = eq.zStart; z >= eq.zEnd; z -= vStep) {
		auto x = getXByEquation(eq, z);
		auto nowLineP = P(x, z);
		if (z == eq.zStart) {
			linePoints << nowLineP;
			lastP = nowLineP;
			continue;
		}
		length += nowLineP.distanceTo(lastP);
		if (length >= segLen * m + segInterval * m && length <= segLen * (m + 1) + segInterval * m)// [0,1*LEN], [2*LEN, 3*LEN], [4*LEN, 5*LEN],... => m:0,2,4,...
			linePoints << nowLineP;
		else if (length > segLen * (m + 1) + segInterval * m) {
			if (linePoints.size() >= 2)// al lease two segments can be drawn with triangle mode
				outVerticesList << linePoints;
			linePoints.clear();
			++m;
		}
		lastP = nowLineP;
	}
	if (linePoints.size() >= 2)// last left data
		outVerticesList << linePoints;
}

P GeoUtil::getPointOnVector(const P& vectorAcrossPoint, float vectorEulerRotX, float D, bool rayFromP) {
	if (vectorEulerRotX < 0) vectorEulerRotX += C_PI;
	P newP;
	newP.x = vectorAcrossPoint.x + std::cos(vectorEulerRotX) * D * (rayFromP ? 1 : -1);
	newP.z = vectorAcrossPoint.z - std::sin(vectorEulerRotX) * D * (rayFromP ? 1 : -1);
	return newP;
}

void GeoUtil::createEllipse(CVector<P>& outVertices, const P& center, float hAxis, float vAxis, int composedPointCnt) {/** XZ-Plane */
	const float StepAngle = C_PI * 2 / composedPointCnt;
	outVertices << center;
	for (int i = 0; i <= composedPointCnt; i++) {
		float theta = i * StepAngle;
		float x = (hAxis / 2) * std::cos(theta) + center.x;
		float z = -(vAxis / 2) * std::sin(theta) + center.z;
		outVertices << P(x, z);
	}
}

void GeoUtil::createEllipseXY(CVector<glm::vec3>& outVertices, const glm::vec3& center, float hAxis, float vAxis, int composedPointCnt) {
	CVector<P> outVerticesXZ;
	createEllipse(outVerticesXZ, P(center.x, -center.y), hAxis, vAxis, composedPointCnt);
	for (auto& p : outVerticesXZ)
		outVertices << glm::vec3(p.x, -p.z, center.z);
}

void GeoUtil::createLineEllipse(CVector<P>& outVertices, const LineEq& lineEq, float centerZ, float hAxis, float vAxis, HDir hdir) {
	// Just cover the upper route
	bool isTowardsLeft = HDir::Left == hdir;
	const float OFFSET_X = (isTowardsLeft ? 1 : -1) * Config::Line_Width / 2;
	const float HSemiAxis = hAxis / 2;
	const float VSemiAxis = vAxis / 2;
	auto centerX = getXByEquation(lineEq, centerZ);
	for (float z = centerZ + VSemiAxis; z >= centerZ - VSemiAxis; z -= Config::Line_PointsZStep) {
		float lineX = getXByEquation(lineEq, z);
		float ellipseX = centerX + HSemiAxis * sqrt(1 - pow(z - centerZ, 2) / pow(VSemiAxis, 2)) * (isTowardsLeft ? -1 : 1);
		float newEllipseX = ellipseX + (lineX - centerX);
		auto ellipseP = P(newEllipseX + OFFSET_X, z);
		auto lineP = P(lineX + OFFSET_X, z);
		outVertices << lineP << ellipseP;
	}
}

bool GeoUtil::createLineArrow(CVector<P>& outVertices, const LineEq& eqL, const LineEq& eqR, float maxTopZ, float zhicheWidth, bool isNotMiddle) {
	const float ZStart = Config::Marker_DclcArrow_ZStart;
	float ZRectEnd = Config::Marker_DclcArrow_ZRectEnd;
	const float ZBezierEnd = Config::Marker_DclcArrow_ZBezierEnd;
	const float RectW =  zhicheWidth * Config::Marker_DclcArrow_RectWByZhicheWTimes;
	float offsetWPercent = Config::Marker_DclcArrow_OffsetWPercentage;
	// Part1: Bottom Rect
	const auto& rectBL = P(-RectW / 2, ZStart);
	const auto& rectBR = P(-rectBL.x, rectBL.z);
	P rectTL, rectTR;
	outVertices << rectBL << rectBR;
	if (isNotMiddle) {
		rectTL = P(rectBL.x, rectBL.z - std::abs(ZStart - ZRectEnd));
		rectTR = P(-rectTL.x, rectTL.z);
		outVertices << rectTL << rectTR;
	} else {
		ZRectEnd = ZStart;
		rectTL = rectBL;
		rectTR = rectBR;
	}
	// Part2: Bezier#Between Rect and Top-Arrow
	const int BezierPointsNum = 30;
	const float BezierCtrlZDelta = std::abs(ZRectEnd - ZBezierEnd) / 2;
	float lx, lDx, rx, rDx;
	getByEquation(eqL, ZBezierEnd, &lx, &lDx);
	getByEquation(eqR, ZBezierEnd, &rx, &rDx);
	const auto& ZBezierEndLaneW = std::abs(lx - rx);
	if (ZBezierEndLaneW < zhicheWidth)// lane too narrow
		return false;
	const float OffsetWPercentIncrement = 0.01;
	float offsetH = ZBezierEndLaneW * offsetWPercent;
	float arrowBottomW = ZBezierEndLaneW - 2 * offsetH;
	while (arrowBottomW > RectW) {// lane too wide
		offsetWPercent += OffsetWPercentIncrement;
		offsetH = ZBezierEndLaneW * offsetWPercent;
		arrowBottomW = ZBezierEndLaneW - 2 * offsetH;
	}
	const auto& arrowBL = P(lx + offsetH, ZBezierEnd);
	const auto& arrowBR = P(rx - offsetH, ZBezierEnd);
	const auto& arrowBLDx = lDx;
	const auto& arrowBRDx = rDx;
	const auto& c0L = rectTL;
	const auto& c0R = rectTR;
	const auto& c3L = arrowBL;
	const auto& c3R = arrowBR;
	const auto& c1L = P(c0L.x, c0L.z - BezierCtrlZDelta);
	const auto& c1R = P(c0R.x, c0R.z - BezierCtrlZDelta);
	const auto& c2L = getPointOnVector(c3L, std::atan(arrowBLDx), BezierCtrlZDelta, false);
	const auto& c2R = getPointOnVector(c3R, std::atan(arrowBRDx), BezierCtrlZDelta, false);
	CVector<P> bezierVerticesL, bezierVerticesR;
	CVector<P> bezierCL, bezierCR;
	bezierCL << c0L << c1L << c2L << c3L;
	bezierCR << c0R << c1R << c2R << c3R;
	createBezierPoints(bezierVerticesL, bezierCL, BezierPointsNum);
	createBezierPoints(bezierVerticesR, bezierCR, BezierPointsNum);
	for (int i = 0; i < bezierVerticesL.size(); i++) {
		auto& bezierL = bezierVerticesL[i];
		auto& bezierR = bezierVerticesR[i];
		if (bezierL.x > bezierR.x)
			return false;
		outVertices << bezierL << bezierR;
	}
	// Part3: Top-Arrow
	const int ArrowPointsNum = 50;
	const float Delta = std::abs(ZBezierEnd - maxTopZ) / (ArrowPointsNum - 1);
	for (int k = 0; k < ArrowPointsNum; k++) {
		float z = ZBezierEnd - k * Delta;
		float arrowXL = getXByEquation(eqL, z);
		float arrowXR = getXByEquation(eqR, z);
		if (arrowXL > arrowXR)
			return false;
		float offsetH = std::abs(arrowXL - arrowXR) * offsetWPercent;
		outVertices << P(arrowXL + offsetH, z) << P(arrowXR - offsetH, z);
	}
	return true;
}

void GeoUtil::createDotMatrixRoadBg(CVector<P>& outVertices, const CVector<LineEq>& eqsFromLToR) {
	ASSERT(eqsFromLToR.size() >= 2, "should have at least 2 lineEq!");
	for (int i = 0; i < eqsFromLToR.size() - 1; i++)
		createRoadBg(outVertices, eqsFromLToR[i], eqsFromLToR[i + 1], RoadBgStyle::DotMatrix);
}

/**
* @warning if using index to draw, although seems good, but in many embed device,
*          because of too many standalone triangles form a too large area, may lead to drawing-wrong-artifacts!
*/
void GeoUtil::createFilledRoadBg(CVector<CVector<P>>& outVerticesList, const CVector<LineEq>& eqsFromLToR) {
	ASSERT(eqsFromLToR.size() >= 2, "should have at least 2 lineEq!");
	for (int i = 0; i < eqsFromLToR.size() - 1; i++) {
		CVector<P> outVertices;
		createRoadBg(outVertices, eqsFromLToR[i], eqsFromLToR[i + 1], RoadBgStyle::Fill);
		if (outVertices.size() < 3) continue;
		outVerticesList << outVertices;
	}
}

void GeoUtil::createRoadBg(CVector<P>& outVertices, const LineEq& eqL, const LineEq& eqR, RoadBgStyle style) {// above maxTopZ is meaningless, because of too small in 3D space
	float zStart = std::min(eqL.zStart, eqR.zStart);
	float zEnd = std::max(eqL.zEnd, eqR.zEnd);

	float IntervalV;
	if (style == RoadBgStyle::DotMatrix)
		IntervalV = Config::Marker_RoadBg_DotIntervalV;
	else if (style == RoadBgStyle::Fill)
		IntervalV = Config::Marker_RoadBg_PointsZStep;

	const auto& LineW = Config::Line_Width;
	const auto& IntervalH = Config::Marker_RoadBg_DotIntervalH;

	int pointsNum = std::abs(zStart - zEnd) / IntervalV + 1;
	for (int k = 0; k < pointsNum; k++) {
		float z = zStart - k * IntervalV;
		auto lx = getXByEquation(eqL, z);
		auto rx = getXByEquation(eqR, z);
		if (style == RoadBgStyle::DotMatrix) {
			auto centerX = (lx + rx) / 2;
			const auto EdgeDelta = LineW * 2;
			int lPartCnt = (centerX - lx - EdgeDelta) / IntervalH;
			int rPartCnt = (rx - centerX - EdgeDelta) / IntervalH;
			outVertices << P(centerX, z);
			for (int i = 0; i < lPartCnt; i++)
				outVertices << P(centerX - IntervalH * (i + 1), z);
			for (int i = 0; i < rPartCnt; i++)
				outVertices << P(centerX + IntervalH * (i + 1), z);
		} else if (style == RoadBgStyle::Fill)
			outVertices << P(lx, z) << P(rx, z);
	}
}

/**
* @note DrawMode: No Culling Face
* 16 -- 15 -- 17   ...Level #5 [A={3(i-1),3i,3i+1}; B={3(i-1),3i+1,3i-2}; C={3(i-1),3i,3i+2}; D={3(i-1),3i+2,3i-1}], i = i + IntervalCnt
* 13 -- 12 -- 14   ...Level #4 [3n,3(n+1),3(n+1)+1; 3n,3(n+1)+1,3(n+1)-2; 3n,3(n+1),3(n+1)+2; 3n,3(n+1)+2,3(n+1)-1]
* 10 --  9 -- 11   ...Level #3 [9,12,13; 9,13,10; 9,12,14; 9,14,11]
* 
*  7 --  6 -- 8    ...Level #2 [3,6,7; 3,7,4; 3,6,8; 3,8,5]
*  4 --  3 -- 5    ...Level #1 [0,3,4; 0,4,1; 0,3,5; 0,5,2]
*  1 --  0 -- 2    ...Level #0
*/
void GeoUtil::createRoadEdge(CVector<glm::vec3>& outVertices, CVector<UInt>& outIndices, const LineEq& eq, float edgeW, float edgeH, float vOffset) {
	bool isOnLeftPart = eq.C[0] > 0;
	const auto UpY = Config::Y_Line + edgeH;
	const auto SegLen = Config::Line_RoadEdgeDashed_SegLen;
	const auto SegInterval = Config::Line_RoadEdgeDashed_SegInterval;
	auto ZStep = Config::Line_PointsZStep;
	if (SegInterval < ZStep)// the gap is shorter than PointsZStep, need more pricise
		ZStep = SegInterval;
	CVector<CVector<P>> cornerPointsList;
	createOneDimensionalDashedLinePoints(cornerPointsList, eq, SegLen, SegInterval, ZStep, vOffset);// Mid-Corner-Points
	int i = 0;
	for (int n = 0; n < cornerPointsList.size(); n++) {
		auto& cornerPoints = cornerPointsList[n];
		for (int m = 0; m < cornerPoints.size(); m++) {
			const auto& cornerP = cornerPoints[m].toP3D(UpY);
			const auto& upP = glm::vec3(cornerP.x + edgeW * (isOnLeftPart ? -1 : 1), cornerP.y, cornerP.z);
			const auto& sideP = glm::vec3(cornerP.x, Config::Y_Line, cornerP.z);
			outVertices << cornerP << upP << sideP;
			if (m >= 1) {
				++i;
				outIndices << 3*(i-1) << 3*i << 3*i+1;
				outIndices << 3*(i-1) << 3*i+1 << 3*i-2;
				outIndices << 3*(i-1) << 3*i << 3*i+2;
				outIndices << 3*(i-1) << 3*i+2 << 3*i-1;
			}
		}
		++i;
	}
	// add head square make it closed alike
	if (outVertices.size() > 2) {
		auto p = glm::vec3(outVertices[1].x, outVertices[2].y, outVertices[1].z);
		outVertices << p;
		auto indexP = outVertices.size() - 1;
		if (isOnLeftPart)// meaningless(culling face disabled)
			outIndices << 0 << 1 << indexP << 0 << indexP << 2;
		else
			outIndices << 0 << 2 << indexP << 0 << indexP << 1;
	}
}

void GeoUtil::createRoadEdge(CVector<glm::vec3>& outVertices, const LineEq& eq, float edgeW, float edgeH) {
	bool isOnLeftPart = eq.C[0] > 0;
	const auto UpY = Config::Y_Line + edgeH;
	CVector<P> cornerPoints;
	createOneDimensionalSolidLinePoints(cornerPoints, eq);
	for (int m = 0; m < cornerPoints.size(); m++) {
		const auto& cornerP = cornerPoints[m].toP3D(UpY);
		const auto& upP = glm::vec3(cornerP.x + edgeW * (isOnLeftPart ? -1 : 1), cornerP.y, cornerP.z);
		outVertices << cornerP << upP;
	}
	for (int m = cornerPoints.size() - 1; m >= 0; m--) {
		const auto& cornerP = cornerPoints[m].toP3D(UpY);
		const auto& sideP = glm::vec3(cornerP.x, Config::Y_Line, cornerP.z);
		outVertices << cornerP << sideP;
	}
	// add head square make it closed alike
	auto secondP = outVertices.at(1);
	outVertices << secondP << glm::vec3(secondP.x, Config::Y_Line, secondP.z);
}

void GeoUtil::createDashedRect(CVector<P>& outVertices, CVector<UInt>& outIndices, const P& rectCenter, float rectWidth, float rectLength, float dashLen, float dashBold) {
	auto topCenter = P(rectCenter.x, rectCenter.z - rectLength / 2);
	auto bottomCenter = P(rectCenter.x, rectCenter.z + rectLength / 2);
	auto leftCenter = P(rectCenter.x - rectWidth / 2, rectCenter.z);
	auto rightCenter = P(rectCenter.x + rectWidth / 2, rectCenter.z);
	
	CVector<P> horizonCenters, vertCenters;
	horizonCenters << topCenter << bottomCenter;
	vertCenters << leftCenter << rightCenter;

	int i = 1;
	while(true) {
		float tlPartX = topCenter.x - 2 * i * dashLen;// top left part
		float trPartX = topCenter.x + 2 * i * dashLen;// top right part
		if (tlPartX - dashLen / 2 < leftCenter.x)
			break;
		horizonCenters << P(tlPartX, topCenter.z) << P(trPartX, topCenter.z) << P(tlPartX, bottomCenter.z) << P(trPartX, bottomCenter.z);
		++i;
	}

	// last 4 corners
	const float CornerDashLenRatio = 0.4;
	const float CornerDashLen = dashLen * CornerDashLenRatio;
	const float Offset = CornerDashLen / 2;
	auto tlCorner = P(leftCenter.x + Offset, topCenter.z);
	auto trCorner = P(rightCenter.x - Offset, topCenter.z);
	auto blCorner = P(leftCenter.x + Offset, bottomCenter.z);
	auto brCorner = P(rightCenter.x - Offset, bottomCenter.z);
	horizonCenters << tlCorner << trCorner << blCorner << brCorner;

	i = 1;
	while(true) {
		float ltPartZ = leftCenter.z - 2 * i * dashLen;// left top part
		float lbPartZ = leftCenter.z + 2 * i * dashLen;// left bottom part
		if (ltPartZ - dashLen / 2 < topCenter.z)
			break;
		vertCenters << P(leftCenter.x, ltPartZ) << P(leftCenter.x, lbPartZ) << P(rightCenter.x, ltPartZ) << P(rightCenter.x, lbPartZ);
		++i;
	}

	for (i = 0; i < horizonCenters.size(); i++) {
		const auto& C = horizonCenters[i];
		float _dashLen = dashLen;
		if (i >= horizonCenters.size() - 4)// last 4 corners
			_dashLen = CornerDashLen;
		auto tl = P(C.x - _dashLen / 2, C.z - dashBold / 2);// top-left
		auto tr = P(C.x + _dashLen / 2, C.z - dashBold / 2);// top-right
		auto bl = P(C.x - _dashLen / 2, C.z + dashBold / 2);// bottom-left
		auto br = P(C.x + _dashLen / 2, C.z + dashBold / 2);// bottom-right
		outVertices << tl << tr << bl << br;
	}
	for (i = 0; i < vertCenters.size(); i++) {
		const auto& C = vertCenters[i];
		auto tl = P(C.x - dashBold / 2, C.z - dashLen / 2);// top-left
		auto tr = P(C.x + dashBold / 2, C.z - dashLen / 2);// top-right
		auto bl = P(C.x - dashBold / 2, C.z + dashLen / 2);// bottom-left
		auto br = P(C.x + dashBold / 2, C.z + dashLen / 2);// bottom-right
		outVertices << tl << tr << bl << br;
	}

	for (i = 0; i < outVertices.size() / 4; i++) {
		int k = 4 * i;
		outIndices << k + 1 << k << k + 2;
		outIndices << k + 1 << k + 2 << k + 3;
	}
}

/** @note if the infinite is close to 90 degrees, and the last point drawing is limited by accuracy */
void GeoUtil::createBezierPoints(CVector<P>& outVertices, const CVector<P>& C, int pointsNum) {// may pointsNum = 30
	float delta = 1.0 / pointsNum;
	for (int k = 0; k < pointsNum; k++) {
		float t = k * delta;// z
		auto p = getBezierPoint(C, t);
		outVertices << p;
	}
}

/**
 *  P(t) = sigma(i=0,3)biBi(t)
 *       = b0B0(t) + b1B1(t) + b2B2(t) + b3B3(t)
 *       = b0*(1-t)^3 + b1*3t(1-t)^2 + b2*3t^2(1-t) + b3*t^3
 * 
 *  P'(t) = 3(b1-b0) + 2(3b0 - 6b1 + 3b2)t + 3(-b0 + 3b1 - 3b2 + b3)t^2
 *  t: 0 ~ 1
 */
P GeoUtil::getBezierPoint(const CVector<P>& C, float t) {
	ASSERT(t >= 0 && t <= 1, "t must be in range of 0 ~ 1");
	float factor;
	P p(0, 0);
	// Calculate the weighted sum of the control points at time t as the next point of the curve
	for (int i = 0; i < 4; ++i) {
		switch (i) {
			case 0: factor = (1 - t) * (1 - t) * (1 - t); break;// (1-t)^3
			case 1: factor = 3 * t * (1 - t) * (1 - t); break;// 3t(1-t)^2
			case 2: factor = 3 * t * t * (1 - t); break;// 3t^2(1-t)
			case 3: factor = t * t * t; break;// t^3
		}
		p += C[i] * factor;
	}
	return p;
}

/**
 *@brief check if a point is on the ray's CCW sweeping forward way side (following the right-hand rule)
 *@param point check it's side relative to the ray
 *@param rayFromPoint the start point to form a ray
 *@param rayToPoint the end point to form a ray
 *@return true: the point is on the ray's CCW sweeping forward way side
 */
bool GeoUtil::checkIfOnRayCCWSweepingSide(const P& point, const P& rayFromPoint, const P& rayToPoint) {
	glm::vec3 rayFrom(rayFromPoint.x, 0, rayFromPoint.z);
	glm::vec3 rayTo(rayToPoint.x, 0, rayToPoint.z);
	glm::vec3 p(point.x, 0, point.z);

	glm::vec3 ray = rayTo - rayFrom;
	glm::vec3 pvec = p - rayFrom;

	glm::vec3 result = glm::cross(ray, pvec);
	return result.y > 0;// sin(alpha) > 0
}

glm::mat4 GeoUtil::createPerspectiveMatrix(float fovy, float aspect, float near, float far) {
	auto projection = glm::mat4(0);
	float tanHalfFOV = tan(fovy / 2);
	projection[0][0] = 1 / (aspect * tanHalfFOV);
	projection[1][1] = 1 / tanHalfFOV;
	projection[2][2] = -(far + near) / (far - near);
	projection[2][3] = -1;
	projection[3][2] = -(2 * far * near) / (far - near);
	return projection;
}

P GeoUtil::mapTo3DXZPlane(int screenX, int screenY, int screenW, int screenH, const glm::mat4& viewMatrix, const glm::mat4& projectionMatrix) {
	// 1. Normalized Device Coordinates (NDC:[-1,1], bottom-left=(-1,-1), top-right=(1,1))
	float ndc_x = (2.0f * screenX) / screenW - 1.0f;// Screen[0,screenW] map to NDC[-1,1]
	float ndc_y = 1.0f - (2.0f * screenY) / screenH;// Screen[0,screenH] map to NDC[1,-1] (openGL Y is up, screen Y is down)
	// 2. Crop spatial coordinates
	glm::vec4 clip_coords(ndc_x, ndc_y, -1.0f, 1.0f);// z = -1 => near-plane
	// 3. View space coordinates
	glm::mat4 P_inv = glm::inverse(projectionMatrix);
	glm::vec4 view_coords = P_inv * clip_coords;// to turn clip-space to view-space
	view_coords /= view_coords.w;// convert to non-homogeneous coordinates, get real view-space coordinates
	// 4. World spatial coordinates
	glm::mat4 V_inv = glm::inverse(viewMatrix);
	glm::vec3 world_coords = glm::vec3(V_inv * view_coords);// to turn view-space to world-space
	// 5. Calculate the direction of the radiation
	auto cameraPos = glm::vec3(V_inv[3]);// the fourth column of the inverse matrix of the view matrix represents the position of the camera in world space
	glm::vec3 ray_direction = glm::normalize(world_coords - cameraPos);// ray direction: from the camera to the point on the screen
	// check if intersection is in front of the camera or ray is almost parallel to the XZ-plane!
	float t = -cameraPos.y / ray_direction.y;
	if (ray_direction.y > 0 || std::isnan(t)) {
		return P();
		/*const float aspect = screenW / screenH;
		const float farPlane = 1 / (P_inv[2][3] + P_inv[3][3]);
		const float fovy = 2 * std::atan(P_inv[1][1]);
		float halfHeight = farPlane * std::tan(fovy / 2);
		float halfWidth = halfHeight * aspect;
		glm::vec3 boundPointCamera = glm::vec3((ndc_x < 0 ? -1 : 1) * halfWidth, 0.0f, -farPlane);// in camera space
		glm::vec3 boundWorld = glm::vec3(V_inv * glm::vec4(boundPointCamera, 1.0f));
		LogI << "screenY:" << screenY << " >> " << (ndc_x < 0 ? "bound-left: " : "bound-right: ") << boundWorld.x << "," << boundWorld.z;*/
	}
	// 6. Calculate the intersection point with the XZ plane
	// the parameter equation of radiation can be expressed as: `intersection` = cameraPos + `t` * ray_direction;
	// `t`: the distance from the camera position, `intersection` is any point on the ray;
	// for XZ-plane, y=0 => cameraPos.y + t * ray_direction.y = 0
	glm::vec3 intersection = cameraPos + t * ray_direction;
	return P(intersection.x, intersection.z);
}